#include "WPILib.h"
//#include "ADXL345_I2C.h"
#include "Math.h"
#include "list"
#include "PIDADXL345_I2C.h"
#include "GearToothPID.h"

//Balancing stuff
#define BALANCETHRESHOLD 		1.0
#define DELTAANGLERATETHRESHOLD 0.5
#define MAXBALANCEDRIVESPEED	0.5
#define MINBALANCEDRIVESPEED	0.25
#define NUMVALUESTOAVERAGE		3

//Other Stuff
#define JOYSTICKDEADSPACE 		0.05
#define RAMPPUSHERMOTORSPEED	0.50
#define INDEXERWAITPERIOD		0.0

//Motor Speeds Stuff
#define SHOOTERTOPSPEED			0.75
#define SHOOTERBOTTOMSPEED		0.75
#define INDEXERSPEED			0.25
#define FLOORSUCKERSPEED		1.0
#define MINIMUMSHOOTSPEED		0.25
#define BALANCEMODESCALEFACTOR	0.45

//Shooter Speeds
#define MAXSHOOTSPEED			1.0
#define HIGHSHOOTSPEED			0.50
#define LOWSHOOTSPEED			0.40
#define MINSHOOTSPEED			0.30
#define SHOOTERTOPBACKSPIN		0.20

//PID RPM Constants
#define RPMMAXSHOOTSPEED		6000
#define RPMHIGHSHOOTSPEED		4000
#define RPMLOWSHOOTSPEED		2500
#define RPMMINSHOOTSPEED		1000

/* NEVER EVER EVER make I equal to 0!!!!!
 * EVER! (you get 'NaN' not a number with printf)
 */
#define USERPM					false
#define RPMFREEZEIVALUE			0.000000000000001
#define RPMP					0.001
#define RPMI					0.000005
#define RPMD					0.0 //0.5

#define RPMNOCONTROL			0
#define RPMREADINITSETNEARSPEED 1
#define RPMDEADRECKONING		2
#define RPMWRITEINITVALUES		3
#define RPMSTARTPIDCONTROL		4
#define RPMPIDCONTROL			5
#define RPMSTARTRECOVERSHOT		6
#define RPMRECOVERSHOT			7

#define CRIORPMSLOWFILE			"RPMs\\4000.txt"


enum RPMSetPoints
{
	RPMMIN, RPMLOW, RPMHIGH, RPMMAX, RPMNULL
};

enum RPMRange
{
	RPMBELOWMIN, RPMABOVEMAX, RPMINRANGE, RPMRANGEERROR
};

//RPM Counting
//should the averaging continue through until each time the motor gets updated
//or should it be averaged a set number of times?

//the last revolution made is not part of the average, but tells when the last full revolution ends
//so to average 5 full rev's, the following value must be 6
//#define RPMNUMVALUESTOAVERAGE	6	  //how many values to get from the sensor before averaging
//#define RPMUPDATESPEED			0.25  //how often to update the motor outputs


/* Calculate an average RPM every [RPMNUMVCALUESTOAVERAGE] (5) cycles through the Teleop loop
 * Calculate an average of these averages every [RPMUPDATESPEED] (.25) seconds, just before
 *    calculating and setting a new motor speed (to compensate, or sustain)
 * After updating the motors, start the process over again
 */

//Transmission Stuff
//0.0 = full left
//1.0 = full right
#define LOWGEARSERVOPOS 		0.0
#define HIGHGEARSERVOPOS 		1.0
//CHANGE THE ABOVE VALUES

//Victor Motor Set Bounds Stuff
#define MAXPWM 				210
#define MAXDEADBAND			138
#define CENTERPWM			132
#define MINDEADBAND			127
#define MINPWM				56

//Joystick Buttons
#define STICK_BUTTON_1 		1
#define STICK_BUTTON_2 		2
#define STICK_BUTTON_3 		3
#define STICK_BUTTON_4 		4
#define STICK_BUTTON_5 		5
#define STICK_BUTTON_6 		6
#define STICK_BUTTON_7 		7
#define STICK_BUTTON_8 		8
#define STICK_BUTTON_9 		9
#define STICK_BUTTON_10 	10
#define STICK_BUTTON_11 	11
#define STICK_BUTTON_12 	12

//xBox Digital Buttons
#define XBOX_A 			1
#define XBOX_B 			2
#define XBOX_X 			3
#define XBOX_Y 			4
#define XBOX_LBUMP 		5
#define XBOX_RBUMP 		6
#define XBOX_BACK 		7
#define XBOX_START 		8
#define XBOX_LSTICK 	9
#define XBOX_RSTICK 	10

//xBox Analog Controls
#define XBOX_LSTICK_X 	1
#define XBOX_LSTICK_Y 	2
#define XBOX_TRIGGERS	3	//-1 = right; +1 = left
#define XBOX_RSTICK_X 	4
#define XBOX_RSTICK_Y 	5

/*
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */ 
class RobotDemo : public SimpleRobot
{
	Victor driveFLeft;
	Victor driveBLeft;
	Victor driveFRight;
	Victor driveBRight;
	RobotDrive myRobot; 		// robot drive system
	Servo driveShiftLeft;		// used to change gears on both transmissions
	Servo driveShiftRight;
	Joystick leftStick; 		// left joystick
	Joystick rightStick;		// right joystick
	Joystick xbox; 				// xbox controller
	Victor floorSucker; 		// picks up balls on the floor
	Victor indexer; 			// spins the tri-stop to release a ball to shoot
	Victor shooterTop;			// the top flywheel motor
	Victor shooterBottom;		// the bottom flywheel motor
	Victor rampArm;				// the motor that moves the ramp arm
	ADXL345_I2C acc; 			// to balance
	Gyro gyro;					// gyroscope
	DigitalInput RampLimitHome;	// home limit switch, arm all the way up
	DigitalInput RampLimitOut;	// out limit switch, arm fully extended
	DigitalInput indexerSensor;	// photosensor to detect the feeder tri-arm thingy
	DigitalInput AutoModeSwitch;// used to change the autonomous mode
	DigitalInput AutoSpinAround;// if the switch is enabled, spins the robot around to face the middle ramp
	//Servo TestServo;			// used for testing the servo to see if it even responds and works
	//PIDADXL345_I2C customPID
	bool balanced;				// true if the autobalance feature detects that it is balanced, to stop trying to do so
	DigitalInput encoderTop;
	DigitalInput encoderBot;
	//Encoder RPMTopEncoder;
	//Encoder RPMBotEncoder;
	GearToothPID RPMGearToothTop;
	GearToothPID RPMGearToothBot;
	PIDController RPMTopPID;
	PIDController RPMBotPID;
	
	float initRPM4000;
	int RPMstate;
	RPMSetPoints setpoint;
	//AxisCamera &camera;

public:
	RobotDemo(void):
		driveFLeft(1),
		driveBLeft(2),
		driveFRight(3),
		driveBRight(4),
		myRobot(driveFLeft, driveBLeft, driveFRight, driveBRight), // y-splitting each to two motor controllers (4 total) for room for 
		driveShiftLeft(2, 9), //digital module must be in 2 or 6, where 2 is '1' and 6 is '2'
		driveShiftRight(2, 10), //CRIO does NOT determine automatically, and explicitely telling it it is in '6' will not work 
		leftStick(1),
		rightStick(2),
		xbox(3),
		floorSucker(5),		//pick up ball from floor
		indexer(6),			//rotating ball feeder
		shooterTop(7),		// top flywheel
		shooterBottom(8),	// bottom flywheel
		rampArm(9),			// ramp arm motor
		
		//toShoot(1,2,false,CounterBase::k1X) // <-- This here sensor ain't workin' right
		//toShoot(1),
		acc(SensorBase::GetDefaultDigitalModule(), ADXL345_I2C::kRange_4G),			//accel. value
		gyro(1),
		//accY(2),			//accel. y value
		RampLimitHome(3),	//ramp pusher limit switch up
		RampLimitOut(2),	//ramp pusher limit switch down
		indexerSensor(4),
		AutoModeSwitch(5),
		AutoSpinAround(6),
		encoderTop(2, 3),
		encoderBot(2, 4),
		//RPMTopEncoder(2, 4, 2, 11, false, Encoder::k1X),
		//RPMBotEncoder(2, 8, 2, 12, false, Encoder::k4X),
		RPMGearToothTop(encoderTop, false),
		RPMGearToothBot(encoderBot, false),
		RPMTopPID(RPMP, RPMI, RPMD, &RPMGearToothTop, &shooterTop),
		RPMBotPID(RPMP, RPMI, RPMD, &RPMGearToothBot, &shooterBottom)
		//TestServo(2, 3)
		//camera(AxisCamera::GetInstance("192.168.0.90"))
	{
		myRobot.SetExpiration(0.1);
		driveFLeft.SetBounds(MAXPWM, MAXDEADBAND, CENTERPWM, MINDEADBAND, MINPWM);
		driveBLeft.SetBounds(MAXPWM, MAXDEADBAND, CENTERPWM, MINDEADBAND, MINPWM);
		driveFRight.SetBounds(MAXPWM, MAXDEADBAND, CENTERPWM, MINDEADBAND, MINPWM);
		driveBRight.SetBounds(MAXPWM, MAXDEADBAND, CENTERPWM, MINDEADBAND, MINPWM);
		driveShiftLeft.SetAngle(HIGHGEARSERVOPOS);
		driveShiftRight.SetAngle(HIGHGEARSERVOPOS);
		balanced = false;
		
		RPMTopPID.SetInputRange(0.0, 10000.0);
		RPMTopPID.SetOutputRange(0.0, 1.0);
		RPMTopPID.SetContinuous(false);
		RPMBotPID.SetInputRange(0.0, 10000.0);
		RPMBotPID.SetOutputRange(0.0, 1.0);
		RPMBotPID.SetContinuous(false);
		//camera = AxisCamera::GetInstance("10.8.34.11");
		
		initRPM4000 = 0.4;
		RPMstate = RPMNOCONTROL;
		setpoint = RPMNULL;
	}

	
//	/*
//	 * BALANCE TESTING
//	 */
//	void balanceBot(float component)
//	{
//		//float xComp = acc.GetAcceleration(ADXL345_I2C::kAxis_X);
//		//float yComp = acc.GetAcceleration(ADXL345_I2C::kAxis_Y);
//		float yComp = component;
//		
//		//driveShiftLeft.SetAngle(LOWGEARSERVOPOS);
//		//driveShiftRight.SetAngle(LOWGEARSERVOPOS);
//		
//		if (fabs(yComp) >= ACCBALANCETHRESHOLD)
//		{
//			//balanced
//			myRobot.SetLeftRightMotorOutputs(0.0, 0.0);
//			balanceStats(component);
//			printf("BALANCED\n");
//		}
//		else
//		{
//			myRobot.ArcadeDrive(-yComp * BALANCESCALEFACTOR, 0.0, false);
//			//myRobot.SetLeftRightMotorOutputs(-yComp * BALANCESCALEFACTOR, -yComp * BALANCESCALEFACTOR);
//			balanceStats(component);
//		}
//	}
	
	void balanceBot(float accXComp, float accYComp, float gyroAngle, float deltaGyro, double deltaGyroTimePassed)
	{
		//get the angle of the robot based on the acceleration in the x and y directions
		float angle = atan2(accYComp, accXComp);
		//use gyrometer to calculate how fast that angle changes, if really fast, we must be tipping
		//otherwise, we most likely still need to continue driving the same direction
		float rateOfAngularChange = deltaGyro / deltaGyroTimePassed;
		//  if angle is less than threshold (close to 0- balanced) and we're not moving then STOP
		
		//check logic. see paper
		if (fabs(angle) < BALANCETHRESHOLD && rateOfAngularChange > DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(-MINBALANCEDRIVESPEED, -MINBALANCEDRIVESPEED);
		}
		else if (fabs(angle) < BALANCETHRESHOLD && rateOfAngularChange < -DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MINBALANCEDRIVESPEED, MINBALANCEDRIVESPEED);
		}
		else if (fabs(angle) < BALANCETHRESHOLD && fabs(rateOfAngularChange) < DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(0.0, 0.0);
		}
		else if (angle < -BALANCETHRESHOLD && rateOfAngularChange > DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(-MINBALANCEDRIVESPEED, -MINBALANCEDRIVESPEED);
		}
		else if (angle < -BALANCETHRESHOLD && rateOfAngularChange < -DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MINBALANCEDRIVESPEED, MINBALANCEDRIVESPEED);
		}
		else if (angle < -BALANCETHRESHOLD && fabs(rateOfAngularChange) < DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MAXBALANCEDRIVESPEED, MAXBALANCEDRIVESPEED);
		}
		else if (angle > BALANCETHRESHOLD && rateOfAngularChange < -DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MINBALANCEDRIVESPEED, MINBALANCEDRIVESPEED);
		}
		else if (angle > BALANCETHRESHOLD && rateOfAngularChange > DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MINBALANCEDRIVESPEED, MINBALANCEDRIVESPEED);
		}
		else if (angle > BALANCETHRESHOLD && fabs(rateOfAngularChange) < DELTAANGLERATETHRESHOLD)
		{
			myRobot.SetLeftRightMotorOutputs(MAXBALANCEDRIVESPEED, MAXBALANCEDRIVESPEED);
		}
		
		printf("acc angle: %f; rate delta angle: %f\n", angle, rateOfAngularChange);
	}
	
	void AutoBalanceTest(float gyroAngle, float deltaGyro, double deltaGyroTimePassed)
	{
		float rateOfAngularChange = deltaGyro / deltaGyroTimePassed;
		
		if (!balanced)
		{
			if (rateOfAngularChange >= DELTAANGLERATETHRESHOLD)
			{
				Timer backTime;
				backTime.Start();
				while (!backTime.HasPeriodPassed(0.1))
				{
					myRobot.SetLeftRightMotorOutputs(-MINBALANCEDRIVESPEED, -MINBALANCEDRIVESPEED);
				}
				backTime.Stop();
				backTime.Reset();
				balanced = true;
			}
			else
			{
				myRobot.SetLeftRightMotorOutputs(MINBALANCEDRIVESPEED, MINBALANCEDRIVESPEED);
			}
		}
	}
	
	float LoadInitDeadReckoning(RPMSetPoints RPMSetPoint)
	{
		printf("LoadInitDeadReckoning\n");
		char* file;
		FILE *CRIOFile;
		float parsedValue;
		
		/* open file, with
		 *	"r" = read
		 *  "w" = write
		 *  "rw" = read/write
		 *  MUST be lowercase
		 */
		
		switch (RPMSetPoint)
		{
			case RPMMIN:
				file = CRIORPMSLOWFILE;
				break;
			case RPMLOW:
				//file = CRIORPMSMINFILE;
				//break;
			case RPMHIGH:
				//file = CRIORPMSHIGHFILE;
				//break;
			case RPMMAX:
				//file = CRIORPMSMAXFILE;
				//break;
			case RPMNULL:
				//break;
			default:
				printf("Invalid RPM SetPoint!\n");
				throw(new exception());
				break;
		}
		
		
		
		CRIOFile = fopen(file, "r");
		
		
		if (CRIOFile == NULL || CRIOFile == 0)
		{
			//if the file could not open...
			printf("ERROR opening file %s\n", file);
		}
		else
		{
			/* code to get file contents and store them in a char array 
			 * char blah2[50];
			 * fgets(blah2, 10, blah);
			 */
			
			//parse the file contents
			if (fscanf(CRIOFile, "%f", &parsedValue) == 1)
			{
				//display parsed value in variable it was stored in
				//printf("%f", parsedValue);
			}
			else
			{
				//error parsing the value from the file
				printf("ERROR parsing file %s contents\n", file);
			}
			
			//printf("%s", blah2);
		}
		
		//close the open file
		fclose(CRIOFile);
		
		return parsedValue;
	}
	
	void WriteInitDeadReckoning(RPMSetPoints RPMSetPoint, float RPMSetPointValue)
	{
		printf("WriteInitDeadReckoning\n");
		char* file;
		FILE *CRIOFile;
		//float parsedValue;
				/* open file, with
		 *	"r" = read
		 *  "w" = write
		 *  "rw" = read/write
		 *  MUST be lowercase
		 */
		
		switch (RPMSetPoint)
		{
			case RPMMIN:
				file = CRIORPMSLOWFILE;
				break;
			case RPMLOW:
				//file = CRIORPMSMINFILE;
				//break;
			case RPMHIGH:
				//file = CRIORPMSHIGHFILE;
				//break;
			case RPMMAX:
				//file = CRIORPMSMAXFILE;
				//break;
			case RPMNULL:
				//break;
			default:
				printf("Invalid RPM SetPoint: %d!\n", RPMSetPoint);
				throw(new exception());
				break;
		}
		
		
		
		CRIOFile = fopen(file, "w");
		
		
		if (CRIOFile == NULL || CRIOFile == 0)
		{
			//if the file could not open...
			printf("ERROR opening file %s\n", file);
		}
		else
		{
			/* code to get file contents and store them in a char array 
			 * char blah2[50];
			 * fgets(blah2, 10, blah);
			 */
			
			//parse the file contents
			/*if (fscanf(CRIOFile, "%f", &parsedValue) == 1)
			{
				//display parsed value in variable it was stored in
				//printf("%f", parsedValue);
			}
			else
			{
				//error parsing the value from the file
				printf("ERROR parsing file %s contents\n", file);
			}*/
			
			//printf("%s", blah2);
			
			fprintf(CRIOFile, "%f", RPMSetPointValue);
		}
		
		//close the open file
		fclose(CRIOFile);
		
		//return parsedValue;
	}
	
	RPMRange GetRPMRangeComparison(RPMSetPoints SetPoint, float CurrentRPMs)
	{
		printf("GetRPMRangeComparison\n");
		float minRPM = 0, maxRPM = 0;
		switch (SetPoint)
		{
			case RPMMIN:
				minRPM = 1000;
				maxRPM = 2000;
				break;
			case RPMLOW:
				minRPM = 2000;
				maxRPM = 3000;
				break;
			case RPMHIGH:
				minRPM = 3000;
				maxRPM = 4000;
				break;
			case RPMMAX:
				minRPM = 4000;
				maxRPM = 5000;
				break;
			case RPMNULL:
				//break;
			default:
				printf("Invalid RPM SetPoint: %d\n", SetPoint);
				throw(new exception());
				break;
		}
		
		if (CurrentRPMs < minRPM)
			return RPMBELOWMIN;
		else if (CurrentRPMs > maxRPM)
			return RPMABOVEMAX;
		else if (CurrentRPMs >= minRPM && CurrentRPMs <= maxRPM)
			return RPMINRANGE;
		
		//if nothing returned so far, a code error has occurred
		return RPMRANGEERROR;
	}
	
//	void balanceStats(float acceleration)
//	{
//		printf("ave-acc: %f; drive: %f\n", acceleration, -acceleration * BALANCESCALEFACTOR);
//	}
	
	/**
	 * Drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void)
	{
		Timer indexTime;
		Timer waitTime;
		bool lastSensorSaw = false;
		bool isFeeding = false;
		//bool indexerWaiting = false;
		int state = -2;
		//int autonomousMode = AutoModeSwitch.Get();
		//bool spinAround = (AutoSpinAround.Get()) ? true : false;
		int autonomousMode = 0;
		//bool spinAround = false;
		
		if (autonomousMode == 0)
		{
			shooterTop.Set(0.4);
			shooterBottom.Set(0.4);
		}
		else if (autonomousMode == 1)
		{
			shooterTop.Set(0.5);
			shooterBottom.Set(0.5);
		}
		
		//Wait(2.5);
		while (IsAutonomous())
		{
			if (state == -2)
			{
				waitTime.Start();
				state = -1;
				printf("state: -2\n");
			}
			else if (state == -1)
			{
				if (waitTime.HasPeriodPassed(2.0))
				{
					waitTime.Stop();
					waitTime.Reset();
					state = 0;
				}
				printf("state: -1\n");
			}
			else if (state == 0)
			{
				isFeeding = true;
				indexer.Set(INDEXERSPEED);
				printf("state: 0\n");
			}
			else if (state == 1)
			{
				waitTime.Start();
				state = 2;
				printf("state: 1\n");
			}
			else if (state == 2)
			{
				if (waitTime.HasPeriodPassed(2.0))
				{
					waitTime.Stop();
					waitTime.Reset();
					state = 3;
				}
				printf("state: 2\n");
			}
			else if (state == 3)
			{
				isFeeding = true;
				indexer.Set(INDEXERSPEED);
				printf("state: 3\n");
			}
			else if (state == 4)
			{
				waitTime.Start();
				state = 5;
				printf("state: 4\n");
			}
			else if (state == 5)
			{
				if (waitTime.HasPeriodPassed(3.0))
				{
					waitTime.Stop();
					waitTime.Reset();
					state = 6;
				}
				printf("state: 5\n");
			}
			else if (state == 6)
			{
				isFeeding = true;
				indexer.Set(INDEXERSPEED);
				printf("state: 6\n");
			}
			else if (state == 7)
			{
				waitTime.Start();
				state = 8;
				printf("state: 7\n");
			}
			else if (state == 8)
			{
				if (waitTime.HasPeriodPassed(2.0))
				{
					waitTime.Stop();
					waitTime.Reset();
					state = 12;
				}
				printf("state = 8\n");
			}
			else if (state == 12)
			{
				shooterTop.Set(0.0);
				shooterBottom.Set(0.0);
				//state++;
				printf("state: 12\n");
				break;
			}
//			else if (state == 7)
//			{
//				if (spinAround)
//				{
//					myRobot.SetLeftRightMotorOutputs(-0.5, -0.5);
//					waitTime.Start();
//					state++;
//				}
//				else
//					break;
//			}
//			else if (state == 8)
//			{
//				if (spinAround && waitTime.HasPeriodPassed(0.5))
//				{
//					myRobot.SetLeftRightMotorOutputs(0.0, 0.0);
//					waitTime.Stop();
//					waitTime.Reset();
//					waitTime.Start();
//					state++;
//				}
//				else
//					break;
//			}
//			else if (state == 9)
//			{
//				if (spinAround && waitTime.HasPeriodPassed(0.25))
//				{
//					myRobot.SetLeftRightMotorOutputs(-0.5, 0.5);
//					waitTime.Stop();
//					waitTime.Reset();
//					waitTime.Start();
//					state++;
//				}
//				else
//					break;
//			}
//			else if (state == 10)
//			{
//				if (spinAround && waitTime.HasPeriodPassed(0.5))
//				{
//					myRobot.SetLeftRightMotorOutputs(0.0, 0.0);
//					waitTime.Stop();
//					waitTime.Reset();
//					break;
//				}
//			}
			
			if (isFeeding)
			{
				//printf("indexer spinning");
				if (indexerSensor.Get() == 1)
				{
					lastSensorSaw = false;
				}
				
				if (indexerSensor.Get() == 0 && !lastSensorSaw)// && !indexerWaiting)
				{	
					lastSensorSaw = true;
					isFeeding = false;
					indexer.Set(0.0);
					//indexerWaiting = true;			
					//lastFeedPressed = false;
					//indexTime.Reset();
					//indexTime.Start();
//					isFeeding = false;
					//break;
					//indexTime.Start();
					if (state == 0)
						state = 1;
					else if (state == 3)
						state = 4;
					else if (state == 6)
						state = 7;
				}
		
//				if (indexTime.HasPeriodPassed(0.0) && indexerWaiting)
//				{
//					isFeeding = false;
//					indexerWaiting = false;
//					//stop the tri-wheel feeder
//					indexer.Set(0.0);
//					indexTime.Stop();
//					indexTime.Reset();
//					//lastSensorSaw = false;
//					//state++;
//					isFeeding = false;
//					//state++;
//					if (state == 0)
//						state = 1;
//					else if (state == 3)
//						state = 4;
//				}
			}
		}
		
//		printf("waiting");
//		Wait(2.0);
//		indexer.Set(INDEXERSPEED);
//		isFeeding = true;
//		//printf("indexing: %d", i + 1);
//		while (isFeeding)
//		{
//			printf("indexer spinning");
//			if (indexerSensor.Get() == 1)
//			{
//				lastSensorSaw = false;
//			}
//			
//			if (indexerSensor.Get() == 0 && !lastSensorSaw)// && !indexerWaiting)
//			{	
//				lastSensorSaw = true;
//				//indexerWaiting = true;			
//				//lastFeedPressed = false;
//				//indexTime.Reset();
//				//indexTime.Start();
//				
//				isFeeding = false;
//				indexer.Set(0.0);
////				indexTime.Start();
//				break;
//			}
//			
////			if (indexTime.HasPeriodPassed(3.5) && indexerWaiting)
////			{
//////				isFeeding = false;
////				indexerWaiting = false;
////				//stop the tri-wheel feeder
//////				indexer.Set(0.0);
////				indexTime.Stop();
////				indexTime.Reset();
////				//lastSensorSaw = false;
////				//state++;
////				isFeeding = false;
////				break;
////				
////			}
////				if (!isFeeding)
////					break;
//		}
//		
//		printf("waiting");
//		Wait(5.0);
//		indexer.Set(INDEXERSPEED);
//		isFeeding = true;
//		//printf("indexing: %d", i + 1);
//		while (isFeeding)
//		{
//			printf("indexer spinning");
//			if (indexerSensor.Get() == 1)
//			{
//				lastSensorSaw = false;
//			}
//			
//			if (indexerSensor.Get() == 0 && !lastSensorSaw)// && !indexerWaiting)
//			{	
//				lastSensorSaw = true;
//				//indexerWaiting = true;			
//				//lastFeedPressed = false;
//				//indexTime.Reset();
//				//indexTime.Start();
//				isFeeding = false;
//				indexer.Set(0.0);
//				break;
//				//indexTime.Start();
//			}
			
//			if (indexTime.HasPeriodPassed(3.5) && indexerWaiting)
//			{
////				isFeeding = false;
//				indexerWaiting = false;
//				//stop the tri-wheel feeder
////				indexer.Set(0.0);
//				indexTime.Stop();
//				indexTime.Reset();
//				//lastSensorSaw = false;
//				//state++;
//				isFeeding = false;
//				break;
//			}
//				if (!isFeeding)
//					break;
//	}
		
		
		shooterTop.Set(0.0);
		shooterBottom.Set(0.0);
		
		//printf("done with autonomous");
	}
	
	/*
	 * Runs the motors with tank drive steering. 
	 */
	void OperatorControl(void)
	{
		myRobot.SetSafetyEnabled(true);
		//int lastRaw;
		
		bool isInFirstGear = true;
		bool balanceMode = false;
		bool lastDirChangePressed = false;
		//bool isFeedShooting = false;
		//bool lastShootPressed = false;
		//bool lastShootPressed = false;
		bool lastFeedPressed = false;
		//bool isShooting = false;
		bool isFeeding = false;
		bool lastArmPusherPressed = false;
		bool isArmPushing = false;
		bool noLimitSwitchesPressed = false;
		bool lastSensorSaw = false;
		bool indexerWaiting = false;
		
		
		bool useRPMoverride = false;
		float nearMotorSpeed;
		//int RPMDeadReckoningLoops = 0;
		
		//if this runs as fast as it can go, 0.1 will max or min out before
		//the RPM's can accurately re-evaluate correctly. 0.01 may not be enough less,
		//so perhaps 0.001 as a step value will be a better 'terrible' control system to start
		const float DEAD_RECKONING_STEPVALUE = 0.01;
		bool RPMFreezeI = false;
		
		int valuesObtained = 0;
		float accXValues[NUMVALUESTOAVERAGE];
		float accYValues[NUMVALUESTOAVERAGE];
		float gyroAngleValues[NUMVALUESTOAVERAGE];
		float lastGyroAverage = -2000.0;
		double lastDeltaAngleTime = -1.0;
		
		//RPM Variables
		//int RPMNumberSeen = 0;	    //counts up to [RPMNUMVALUESTOAVERAGE]
		//int RPMValuesAveraged = 0;	//counts how many times an average has been calculated (to calculate final average)
		//list<double> RPMAverages;	//stores each iteration of 5 values that are averaged
		//Timer RPMTime;				//stores how long each set of 5 passes takes
		//Timer RPMUpdateTime;		//timer starts and updates the motors every [RPMUPDATESPEED] seconds
		
		double ShooterTopSpeed = 0.40, ShooterBottomSpeed = 0.40;
		
		//used to wait after the eye-sensor sees the indexer, so that it lines up to stop the ball correctly
		Timer indexTime;
		//Timer shootTime;
		//Timer armTime;
		
		//used to calculate how fast the rate of change in the angle is (from gyro)
		Timer deltaAngleTime;
		
		//shooterTop.Set(1.0);
		//shooterBottom.Set(1.0);
		
		Timer rampArmTime;
		
		//printf("sensor: 8");
		//printf("acc: %f, %f", acc.GetAcceleration(XAxis), acc.GetAcceleration(YAxis));
		
		
		while (IsOperatorControl())
		{
			//DRIVE
			//if a joystick is below the threshold, don't drive forward
			float leftStickY = fabs(leftStick.GetY());
			float rightStickY = fabs(rightStick.GetY());
			
			//ShooterTopSpeed = LOWSHOOTSPEED;
			//ShooterBottomSpeed = LOWSHOOTSPEED;
			
			if (xbox.GetRawButton(XBOX_Y))
			{
				if (valuesObtained < NUMVALUESTOAVERAGE)
				{
					accXValues[valuesObtained] = acc.GetAcceleration(ADXL345_I2C::kAxis_X);
					accYValues[valuesObtained] = acc.GetAcceleration(ADXL345_I2C::kAxis_Y);
					gyroAngleValues[valuesObtained] = gyro.GetAngle();
					valuesObtained++;
				}
				else
				{
					float averageX = 0.0;
					float averageY = 0.0;
					float averageGyro = 0.0;
					float changeInGyroAngle = 0.0;
					double deltaGyroTimePassed = 0.0;
					for (int i = 0; i < NUMVALUESTOAVERAGE; i++)
					{
						averageX += accXValues[i];
						averageY += accYValues[i];
						averageGyro += gyroAngleValues[i];
					}
					averageX /= NUMVALUESTOAVERAGE;
					averageY /= NUMVALUESTOAVERAGE;
					averageGyro /= NUMVALUESTOAVERAGE;
					if (lastGyroAverage == -2000.0)
					{
						lastGyroAverage = averageGyro;						 
						deltaAngleTime.Start();
						lastDeltaAngleTime = deltaAngleTime.Get();
					}
					else
					{
						deltaGyroTimePassed = deltaAngleTime.Get() - lastDeltaAngleTime;
						changeInGyroAngle -= lastGyroAverage;
						lastGyroAverage = averageGyro;
						deltaAngleTime.Stop();
						deltaAngleTime.Reset();
						deltaAngleTime.Start();
						lastDeltaAngleTime = deltaAngleTime.Get();
					}
					//balanceBot(averageX, averageY, averageGyro, changeInGyroAngle, deltaGyroTimePassed);
					AutoBalanceTest(averageGyro, changeInGyroAngle, deltaGyroTimePassed);
					valuesObtained = 0;
				}
				//balanceBot();
				//printf("acc: %f\n", (acc.GetAcceleration(ADXL345_I2C::kAxis_Y)));//, acc.GetAcceleration(ADXL345_I2C::kAxis_Y));
			}
			else
			{
				if (balanceMode)
				{
					//printf("%f\n", leftStick.GetY());
//					if (leftStickY > JOYSTICKDEADSPACE && rightStickY > JOYSTICKDEADSPACE)
//					{
//						myRobot.TankDrive(leftStick.GetY() * BALANCEMODESCALEFACTOR, rightStick.GetY() * BALANCEMODESCALEFACTOR);
//					}
//					else if (leftStickY < JOYSTICKDEADSPACE && rightStickY > JOYSTICKDEADSPACE)
//					{
//						myRobot.TankDrive(0.0, rightStick.GetY() * BALANCEMODESCALEFACTOR);
//					}
//					else if (leftStickY > JOYSTICKDEADSPACE && rightStickY < JOYSTICKDEADSPACE)
//					{
//						myRobot.TankDrive(leftStick.GetY() * BALANCEMODESCALEFACTOR, 0.0);
//					}
//					else if (leftStickY < JOYSTICKDEADSPACE && rightStickY < JOYSTICKDEADSPACE)
//					{
//						myRobot.TankDrive(0.0, 0.0);
//					}
					
					myRobot.TankDrive(leftStick.GetY() * BALANCEMODESCALEFACTOR, rightStick.GetY() * BALANCEMODESCALEFACTOR);
				}
				else
				{
					//printf("%f\n", leftStick.GetY());
					if (leftStickY >= JOYSTICKDEADSPACE && rightStickY >= JOYSTICKDEADSPACE)
					{
						myRobot.TankDrive(leftStick.GetY(), rightStick.GetY());
					}
					else if (leftStickY <= JOYSTICKDEADSPACE && rightStickY >= JOYSTICKDEADSPACE)
					{
						myRobot.TankDrive(0.0, rightStick.GetY());
					}
					else if (leftStickY >= JOYSTICKDEADSPACE && rightStickY <= JOYSTICKDEADSPACE)
					{
						myRobot.TankDrive(leftStick.GetY(), 0.0);
					}
					else if (leftStickY <= JOYSTICKDEADSPACE && rightStickY <= JOYSTICKDEADSPACE)
					{
						myRobot.TankDrive(0.0, 0.0);
					}
				}
			}
			
			//changes the drive to balanced mode (crazy joystick movements only move the bot a little)
			// or to normal (power the motors to the amount the joysticks are)
			if (rightStick.GetRawButton(STICK_BUTTON_1))
			{
				if (!lastDirChangePressed)
				{
					balanceMode = (balanceMode) ? false : true;
				}
				lastDirChangePressed = true;
			}
			else
			{
				lastDirChangePressed = false;
			}
			
			//right stick, top 1st gear, bottom 2nd gear
			//JOYSTICK CONTROLS
			if (rightStick.GetRawButton(STICK_BUTTON_2))
			{
				//FIRST GEAR
				if (!isInFirstGear)
				{
					//set servo to certain angle
					//0.0 to 1.0
					//use Get() to check what angle actually is
					//when in different physical positions
					
					driveShiftLeft.Set(LOWGEARSERVOPOS);
					driveShiftRight.Set(LOWGEARSERVOPOS);
					isInFirstGear = true;
				}
				//printf("LowGear; L: %f R: %f\n", driveShiftLeft.Get(), driveShiftRight.Get());
			}
			
			if (rightStick.GetRawButton(STICK_BUTTON_3))
			{
				//SECOND GEAR
				if (isInFirstGear)
				{
					//set servo to other certain angle
					//0.0 to 1.0
					
					driveShiftLeft.Set(HIGHGEARSERVOPOS);
					driveShiftRight.Set(HIGHGEARSERVOPOS);
					isInFirstGear = false;
				}
				//printf("HighGear; L: %f R: %f\n", driveShiftLeft.Get(), driveShiftRight.Get());
			}
			
			if (leftStick.GetRawButton(STICK_BUTTON_10))
			{
				//useRPMoverride = true;
			}
			else
			{
				useRPMoverride = false;
			}
			
			useRPMoverride = false;
			
			//XBOX CONTROLS
			if (xbox.GetRawButton(XBOX_B))
			{
				//pick off the floor
				floorSucker.Set(-FLOORSUCKERSPEED);
			}
			else if (xbox.GetRawButton(XBOX_A) && !xbox.GetRawButton(XBOX_B))
			{
				//reverse the ball pick up motor
				floorSucker.Set(FLOORSUCKERSPEED);
			}
			else
			{
				floorSucker.Set(0.0);
			}
			
			//printf("Raw: %d\r", toShoot.Get());
			
			//set shooter speed to the position of the joystick if it is greater than a threshold
//			if (xbox.GetRawAxis(1) > MINIMUMSHOOTSPEED)
//			{
//				ShooterTopSpeed = xbox.GetRawAxis(1);
//				ShooterBottomSpeed = xbox.GetRawAxis(1);
//				printf("shoot: %f", xbox.GetRawAxis(1));
//			}
//			else
//			{
//				ShooterTopSpeed = SHOOTERTOPSPEED;
//				ShooterBottomSpeed = SHOOTERBOTTOMSPEED;
//			}
			
			if (!useRPMoverride)
			{
				/*if (ShooterTopSpeed > 1.0 || ShooterBottomSpeed > 1.0)
				{
					ShooterTopSpeed = LOWSHOOTSPEED;
					ShooterBottomSpeed = LOWSHOOTSPEED;
				}*/
				setpoint = RPMNULL;
				
				if (xbox.GetRawButton(XBOX_RBUMP))
				{
					ShooterTopSpeed = LOWSHOOTSPEED;
					ShooterBottomSpeed = LOWSHOOTSPEED;
				}
				else if (xbox.GetRawButton(XBOX_LBUMP))
				{
					ShooterTopSpeed = HIGHSHOOTSPEED;
					ShooterBottomSpeed = HIGHSHOOTSPEED;
				}
				else if (xbox.GetRawAxis(XBOX_TRIGGERS) == -1)
				{	
					ShooterTopSpeed = MINSHOOTSPEED;
					ShooterBottomSpeed = MINSHOOTSPEED;
				}
				else if (xbox.GetRawAxis(XBOX_TRIGGERS) == 1)
				{
					ShooterTopSpeed = MAXSHOOTSPEED;
					ShooterBottomSpeed = MAXSHOOTSPEED;
				}
				/*else
				{
					//KEEP UNCOMMENTED to keep motor speeds set to last bumper pressed
				}*/
			}
			else
			{
				/*if (ShooterTopSpeed <= 1.0 || ShooterBottomSpeed)
				{
					ShooterTopSpeed = RPMLOWSHOOTSPEED;
					ShooterBottomSpeed = RPMLOWSHOOTSPEED;
				}*/
				
				
				if (xbox.GetRawButton(XBOX_RBUMP))
				{
					ShooterTopSpeed = RPMLOWSHOOTSPEED;
					ShooterBottomSpeed = RPMLOWSHOOTSPEED;
					setpoint = RPMLOW;
				}
				else if (xbox.GetRawButton(XBOX_LBUMP))
				{
					ShooterTopSpeed = RPMHIGHSHOOTSPEED;
					ShooterBottomSpeed = RPMHIGHSHOOTSPEED;
					setpoint = RPMHIGH;
				}
				else if (xbox.GetRawAxis(XBOX_TRIGGERS) == -1)
				{	
					ShooterTopSpeed = RPMMINSHOOTSPEED;
					ShooterBottomSpeed = RPMMINSHOOTSPEED;
					setpoint = RPMMIN;
				}
				else if (xbox.GetRawAxis(XBOX_TRIGGERS) == 1)
				{
					ShooterTopSpeed = RPMMAXSHOOTSPEED;
					ShooterBottomSpeed = RPMMAXSHOOTSPEED;
					setpoint = RPMMAX;
				}
				/*else
				{
					
				}*/
			}
			
			//if (xbox.GetRawButton(XBOX_A))
			//{
				//run shoot motors if not running
				//stop them if they are
				//if (isShooting && !lastShootPressed)
				//{
					//isShooting = false;
					//shooterTop.Set(0.0);
					//shooterBottom.Set(0.0);
				//}
				//else if (!isShooting && !lastShootPressed)
				//{
					//isShooting = true;
					//shooterTop.Set(ShooterTopSpeed);
					//shooterBottom.Set(ShooterBottomSpeed);
				//}
				
//*************************************************
//UNCOMMENT FOR USE IN GAME!!!!!!
			//backspin motor speeds
			/* 
			 * 	add code for that here
			 */
			
			//RPM Averaging
//			if (RPMUpdateTime.HasPeriodPassed(RPMUPDATESPEED))
//			{
//				RPMUpdateTime.Stop();
//				RPMUpdateTime.Reset();
//				double totalAve = 0.0;
//				
//				//loop through as many times as recorded
//				for(int i = 0; i < RPMValuesAveraged; i++)
//				{
//					//get the value at the beginning and add it to the total
//					totalAve += RPMAverages.front();
//					RPMAverages.pop_front();
//					//then remove that from the front (and the next one will now be ready at the front)
//				}
//				
//				// Why doesn't this work!?
//				/*
//				for each (double ave in RPMValues)
//				{
//					totalAve += ave;
//				}
//				
//				*/
//				
//				totalAve /= RPMValuesAveraged;
//				
//				// 
//				//now calculate what it *should* be at, and set the motor
//			}
//			else
//			{
//				if (RPMNumberSeen < RPMNUMVALUESTOAVERAGE)
//				{
//					//keep track of the time and the times the sensor has been seen
//					if (testEncoder.Get() == 1)
//					{
//						//only start the timer on the first iteration
//						if (RPMNumberSeen == 0)
//							RPMTime.Start();
//						
//						RPMNumberSeen++;
//					}
//					
//				}
//				else if (RPMNumberSeen == RPMNUMVALUESTOAVERAGE)
//				{
//					//stop the timer
//					RPMTime.Stop();
//					double tempAverage = RPMNumberSeen / RPMTime.Get();
//					//is this value alright? its not exactly 'RPM's... :/
//					RPMAverages.push_back(tempAverage);
//					//another one added, keep track
//					RPMValuesAveraged++;
//					//start the loop over
//					RPMNumberSeen = 0;
//					RPMTime.Reset();
//				}
//			}
			
			/* TODO:
			   RPM sensing
			 */
			
			if (useRPMoverride)
			{
				if (RPMstate == RPMNOCONTROL)
				{
					//RPMstate = RPMREADINITSETNEARSPEED;
				}
				
				if (GetRPMRangeComparison(setpoint, RPMGearToothTop.PIDGet()) != RPMINRANGE) RPMFreezeI = true;
				else RPMFreezeI = false;
				
				//RPMTopPID.SetSetpoint(ShooterTopSpeed);
				//RPMBotPID.SetSetpoint(ShooterBottomSpeed);
				//RPMTopPID.Enable();
				//RPMBotPID.Enable();
				//shooterBottom.Set(0);
			}
			else
			{
				RPMstate = RPMNOCONTROL;
				//RPMTopPID.Disable();
				//RPMBotPID.Disable();
				
				shooterTop.Set(ShooterTopSpeed);
				shooterBottom.Set(ShooterBottomSpeed);
				//shooterTop.Set(1.0);
				//printf("Shooter: %f\n", ShooterTopSpeed);
			}
			
			
			//WHY WHY WHY DOES THIS RUN!?!?!?!?!?!?!?!?!?!!!!?????
			
			/*switch (RPMstate)
			{
				case RPMREADINITSETNEARSPEED:
					nearMotorSpeed = LoadInitDeadReckoning(setpoint);
					
					shooterTop.Set(nearMotorSpeed);
					//shooterBottom.Set(nearMotorSpeed);
					
					
					//advance the state machine
					RPMstate = RPMDEADRECKONING;
										
					break;
				case RPMDEADRECKONING:
					//RPMDeadReckoningLoops++;
					switch (GetRPMRangeComparison(setpoint, RPMGearToothTop.PIDGet()))
					{
						case RPMABOVEMAX:
							//need to go less, 
							nearMotorSpeed -= DEAD_RECKONING_STEPVALUE;
							if (nearMotorSpeed < 0)
								nearMotorSpeed = 0;
							shooterTop.Set(nearMotorSpeed);
							break;
						case RPMBELOWMIN:
							//need to go more
							nearMotorSpeed += DEAD_RECKONING_STEPVALUE;
							if (nearMotorSpeed > 1.0)
								nearMotorSpeed = 1.0;
							shooterTop.Set(nearMotorSpeed);
							break;
						case RPMINRANGE:
							RPMstate = RPMWRITEINITVALUES;
							break;
						case RPMRANGEERROR:
						default:
							//something is seriously wrong...
							throw(new exception());
							break;
					}
					break;
				case RPMWRITEINITVALUES:
					WriteInitDeadReckoning(setpoint, nearMotorSpeed);
					
					RPMstate = RPMSTARTPIDCONTROL;
					break;
				case RPMSTARTPIDCONTROL:
					//what does the setpoint now need to be?
					//should it be set when the file is written?
					//set the PID values back to normal
					RPMTopPID.SetPID(RPMP, RPMI, RPMD);
					RPMTopPID.SetSetpoint(setpoint);
					//RPMBotPID.SetSetpoint(ShooterBottomSpeed);
					RPMTopPID.Enable();
					//RPMBotPID.Enable();
					
					break;
				case RPMPIDCONTROL:
					if (GetRPMRangeComparison(setpoint, RPMGearToothTop.PIDGet()) != RPMINRANGE)
						RPMstate = RPMSTARTRECOVERSHOT;
					
					break;
				case RPMSTARTRECOVERSHOT:
					//measure time for PID to get back to speed after a ball is fed through
					//freeze i for that period of time; start freeze when button is pressed?
					//for now, frozen from button press to indexer done spinning
					RPMTopPID.SetPID(RPMP, RPMFREEZEIVALUE, RPMD);
					RPMstate = RPMRECOVERSHOT;
					break;
				case RPMRECOVERSHOT:
					if (GetRPMRangeComparison(setpoint, RPMGearToothTop.PIDGet()) == RPMINRANGE) RPMstate = RPMSTARTPIDCONTROL;
					break;
				case RPMNOCONTROL:
				default:
					RPMTopPID.Disable();
					RPMBotPID.Disable();
					break;
			}*/
			
//*************************************************
				//lastShootPressed = true;
			//}
			//else
			//{
				//lastShootPressed = false;
				//shooterTop.Set(0.0);
				//shooterBottom.Set(0.0);
			//}
			
//			if (isShooting)
//			{
//				shooterTop.Set(ShooterTopSpeed);
//				shooterBottom.Set(ShooterBottomSpeed);
//			}
			
			if (xbox.GetRawButton(XBOX_START))
			{
				//indexer
				if (isFeeding && !lastFeedPressed)
				{
					//e-stop feeder
					//isFeeding = false;
					//indexer.Set(0.0);
				}
				else if (!isFeeding)// && !lastFeedPressed)
				{
					isFeeding = true;
					indexer.Set(INDEXERSPEED);
					//indexTime.Start();
				}
				
				//lastFeedPressed = true;
			}
			else
			{
				//lastFeedPressed = false;
			}
			
			if (isFeeding)
			{
				//if we are currently in the process of feeding
				//continue checking the sensor to see if it has reached the
				//mark it needs for a 1/3 rotation.
				if (xbox.GetRawButton(XBOX_BACK))
				{
					isFeeding = false;
				}
				
				if (indexerSensor.Get() == 1)
				{
					lastSensorSaw = false;
				}
				
				if (indexerSensor.Get() == 0 && !lastSensorSaw && !indexerWaiting)
				{	
					lastSensorSaw = true;
					indexerWaiting = true;
					//lastSensorSaw = true;
					//lastFeedPressed = false;
					indexTime.Start();
				}
				
				if (indexTime.HasPeriodPassed(INDEXERWAITPERIOD) && indexerWaiting)
				{
					isFeeding = false;
					indexerWaiting = false;
					//stop the tri-wheel feeder
					indexer.Set(0.0);
					indexTime.Stop();
					indexTime.Reset();
					//lastSensorSaw = false;
				}
			}
			
			
			//start, ramp pusher downer
			if (xbox.GetRawButton(XBOX_X))
			{
				//using two limit switches. one for 'home' and one for 'out'
				//push button, do its thing. push it again, return to previous position.
				if (!lastArmPusherPressed && !isArmPushing && RampLimitHome.Get())
				{
					rampArm.Set(RAMPPUSHERMOTORSPEED);
					isArmPushing = true;
				}
				else if (!isArmPushing && RampLimitOut.Get())
				{
					if (rampArmTime.Get() == 0)
						rampArmTime.Start();
					else if (rampArmTime.HasPeriodPassed(3.0))
					{
						rampArmTime.Stop();
						rampArmTime.Reset();
						rampArm.Set(RAMPPUSHERMOTORSPEED);
						isArmPushing = true;
					}
				}
				else if (!isArmPushing && noLimitSwitchesPressed)
				{
					rampArm.Set(RAMPPUSHERMOTORSPEED);
					isArmPushing = true;
				}
				lastArmPusherPressed = true;
			}
			else
			{
				lastArmPusherPressed = false;
				rampArmTime.Stop();
				rampArmTime.Reset();
			}
			
			//wait for a limit switch to be pressed, after both are not pressed. (i.e. arm is in the middle somewhere)
			if (!RampLimitHome.Get() && !RampLimitOut.Get() && !noLimitSwitchesPressed)
			{
				noLimitSwitchesPressed = true;
			}
			
			if (isArmPushing)
			{
				if (noLimitSwitchesPressed && (RampLimitHome.Get() || RampLimitOut.Get()))
				{
					isArmPushing = false;
					noLimitSwitchesPressed = false;
					rampArm.Set(0.0);
				}
			}
			
			if (xbox.GetRawButton(XBOX_BACK))
			{
				isFeeding = false;
				indexer.Set(-0.15);
			}
			else if (!isFeeding)
			{
				indexer.Set(0.0);
			}
			
			//if (useRPMoverride)
			//	printf("USE RPM's\r");
			//else printf("USE BUTTONS\r");
			
			//printf("RPMState: %d\n", RPMstate);
			
			//printf("\x08%d", armSensor.Get());
			
			//printf("sensor: %d\n", testEncoder.Get()); //binary sensor check
			//printf("encoder: %f\n", testGearTooth.PIDGet()); //custom class check
			//printf("min- %f, max+ %f\n", shooterTop.GetMinNegativePwm(), shooterTop.GetMaxPositivePWM()); //PWM value check
			//printf("PIDT: %f, %f\n", RPMTopPID.Get(), RPMTopPID.GetError());
			//printf("PIDT: %f, %f; PIDB: %f, %f\n", RPMTopPID.Get(), RPMTopPID.GetError(), RPMBotPID.Get(), RPMBotPID.GetError()); //PID controller check
			
			// wait for a motor update time
			Wait(0.005);
		}
	}
};

START_ROBOT_CLASS(RobotDemo);

